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ABSTRACT 

Here  is  a  simple  example  of  control  of  inherently  unstable 
system.     An  inverted  pendulum  pivoted  on  top  of  a  cart  is  to  be 
stabilized  by  applying  force  to  the  cart  through  an  electric  motor. 

In  the  electrical  laboratory  of  the  United  States  Naval  Post- 
graduate School,   a  cart  with  a  stick  pivoted  on  top  of  it  has  been 
built,   tested  and  simulated  with  CDC  1604  digital  computer. 

The  author,    Lieutenant  Mu-yu  Wan  of  the  Chinese  Navy,   wishes 
to  thank  Dr.   Harold  A.    Titus  of  the  United  States  Naval  Postgrad- 
uate School  for  his  patient  assistance  in  this  work  as  thesis  super- 
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1.     Introduction 


It  seems  to  be  interesting  to  provide  artificial  stability  for  an 
inherently  unstable  physical  system.     Some  immediate  questions 
arise,    such  as:    wtjat  are  the  largest  errors  which  can  possibly  be 
corrected  with  a  limited  available  control  force,   and  what  is  the 
best  control  strategy  which  minimizes  the  time  required  and  maxi- 
mizes the  size  of  the  system  errors  which  can  be  corrected. 

In  this  simple  case  here,  an  inverted  pendulum  pivoted  on  top 
of  a  cart  is  to  be  stabilized  by  applying  limited  force  to  the  cart 
through  an  electric  motor.     This  inherently  unstable  system  is 
assumed  to  be  adequately  represented  by  a  set  of  linear  differen- 
tial equations  over  the  range  of  interest.     The  type  of  instability 
is  represented  by  real  non-negative  characteristic  roots.     The 
motor  supply  voltage  is  manipulated,  within  its  allowable  limits, 
as  a  function  of  the  state  variables  of  the  set  of  linear  differential 
equations. 

In  Chapter  3,   an  analog  computer  is  used  to  really  balance 

the  pendulum.     The  general  schematic  of  the  system  is  shown  here. 

control  |  unstable 


/*\ 
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Control 
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iimiter 
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FIG.    (1-1).    General  Schematic 


In  Chapter  4,   the  whole  system  is  simulated  with  CDC  1604 
digital  computer.     Graphs  are  plotted  and  situations  discussed. 

In  Chapter  5,  the  techniquerof  optimal  discrete-time  control 
is  introduced.  The  results  simulated  by  CDC  1604  turned  out  to 
be  successful. 


2.     Uncontrolled  System 

2.    1    Linear  Differential  Equations 

As  shown  in  Fig.(2  - 1  - 1 )  7  the  instantaneous  angle  that  the  pendu- 
lum makes  with  its  unstable  equilibrium  position  is  0,   and  the 
position  of  the  cart  with  respect  to  some  reference  point  on  the 
floor  is|...    The  coordinates  are  shown  with  positive  displacement. 


tt^ti 


J     ynotoT  ty/fage  V 


FIG.    (2-1-1).     System  To  Be  Stabilized 
For  establishing  the  equations  of  motion,   we  define  some  addi- 
tional symbols: 
f        force  applied  to  cart 

f  -      damping  coefficient  including  friction  and  back  e.   m.   f. 
% 

fv      applied  voltage  force  coefficient 


g        acceleration  of  gravity 

M      total  system  effective  mass 

m      mass  of  the  pendulum 

r        distance  of  pendulum  mass  center  from  hinge  line 

T       pendulum  radius  of  gyration  about  hinge  line 

V       v'pltage  applied  to  d.   c.    drive  motor 

The  equations  of  motion  can  be  found  by  consideration  of  the 
following  diagram. 


*  _e_r- 


FIG.    (2-1-2).     Force  Diagram  Showing  Equations  of  Motion 
*    Note:    0  is  very  small. 


The  liniarized  equations  of  motion  are 


2- 


my  9  =  mrg0  -  mr^ 


(2.1.1) 


and 

M£  =  -  mrS  +  f  (2.  1.  2) 

where 

f  =  f|i  +fvV  (2.  1.  3) 

We  have  data  measurements  as  follows: 

g  =  9.8    m/sec2 

m  =  0.  225    kg. 

M  =  6.  0  kg. 

r  =  0.  44  m. 

1  =  length  of  longer  part  from  hinge  =  0.  94  m. 

h  =  length  of  shorter  part  from  hinge  =  0.  035  m. 

i*=  (l2  +  3h2  -  31h)  x  1/3  =  0.  25  m2 

V  =  24  volts  (for  selected  D.    C.    motor) 

The  force  exerted  on  the  cart  is  13.  6  newton  while  the  cart  is 
held  still,   i.  e.  ,    \    =  0,   thus  from  (2.  1.  3) 

fv    =  ±=l^lJh    =0   57    n/n 

v        V  24    v  v 

In  (2.  1.  2),   we  have  mass  of  the  pendulum  much  less  than  mass  of 
the  whole  system,   the  term  "mr9n  can  be  neglected,   given: 

M  %  =  f  =  f;  \  +  fvV  (2.  1.  4) 

Securing  the  pendulum  (0  =  0),  we  run  the  cart  on  the  floor 
and  observe  the  motion  with  a  brush  recorder,  find  the  average 
velocity  and  acceleration  as: 

.  .  \     .  5 


%  =  0.  90  m/sec 


and 


=  0.  83  m/sec 


By  (2.  1.4) 


V 


n'i-lvV 


11.8     n~sec/ 


m. 


Now  (2.  1.  1)  and  (2.  1.  4)  can  be  written  as: 
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T 


L0 . 


f 


5=^H 


v 


Define 

9 

=  qi 

• 

9 

=  *i  = 

92 

I 

~  *i 

i 

Hr 

h 

Then,   we  get  a  set  of  linear  differential  equations  as: 

e,  =  92 

In  matrix  form: 


«1 ' 

82 

- 

\ 

S\ 

0        10       0 

a0   o-fi 

0       0       0        1 


0        0        0 


ii 

M 


* 

91 

0 

92 

+ 

J1 

t, 

0 

1, 

g 

Vfv 


n 


J 


(2.1.1) 
(2.1.4) 


(2.1.  5) 


Substituting  numberical  values: 


81 

92 


0 

1 

17.2 

0 

0 

0 

0 

0 

0         0 

0  3.  5 
0         1 

0  -2 


f 

- 

91 

0 

X 

92 

+ 

-17.  2 

k 

0 

^2 

9.  8 

Vfv 


(2.  L  5) 


Or,   by  defining  some  new  matrix  names,   and  x*  s    as  the  name  of 
state  variables: 

(2.1.  6) 

(2.1.  7) 


(2.1.8) 


x  =    [a]    x    +  c_   u 

re 

Vfv 
u  "  Mg 

[4- 

0         1 

0 

0 

[A]- 

it    0 

?2 

0 

~nfl 

0        0, 

0 

1 

0       0 

0 

r         • 

el 

0 

• 

X      = 

62 

X      = 

92 
h 

c_  = 

.11 

r2 
0 

g 

< 

2.  2    Transformation  to  Canonical  Form 

Assuming  in  our  linear  transformation  matrix  [Al    there  is  some 
eigenvector  v  associated  with  eigenvalue  A  : 

[A]  v  =  7v  v 


[A  -  A   I]  v  =  0 


or 


|A  -  A.l|    =  0 
By  (2.1.  8) 

-;v       1 


r 

0 
0 


-A. 

0 

0 


0 

0 

-A. 


o       #-A 


=  0 


There  comes  the  characteristic  equation: 


The  eigenvalues  are: 


V 

-flt 

=  -  4. 

15 

A2  = 

+   i^ 

=  +  4. 

15 

A3  = 

J 

0 

^+  = 

n        2 

/ 


where    V    ^    0 


Only  A   and  A  j  are  non-negative  or  unstable. 

With  these  eigenvalues  all  distinct,   one  can  always  find  a  new  set 

of  state  variables: 

8 


y4 

related  to    x    by  the  transformation 

Z    -  O]  2£ 
such  that  the  system  of  equations  (2.  1.  6)  transforms  to: 


(2.  2.1) 


S^< 


/V. 


0 


0 


7v 


/V 


1+    Du 


(2.  2.  2) 


with  the  4x4  matrix  [<qj  and  vector    D    given  later. 


From  (2.  2. 1) 
-1 


2£    ■[»! 


(2.  2.  3) 


Let 


[Gr]-1- 


§12 
§22 
§32 


e13 
§23 


§14 
§24 
§34 


§11 

§21 

§31  &32  e33 

§41  §42  §43  §44 

The  transformation  matrix  [Cy]    is  not  unique,   but  a  convenient 

form  for  this  problem  can  be  found  by  four  column  vectors,   all  are 

eigenvectors  associated  with  one  eigenvalue  respectively. 
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Define 


-1 


L°r    £  [4  ^2   ^3    v4] 

Where 

[A  -  A.    I]     v.     =  0  i    =1,    2,    3,    4 

But   Aj=    -Az  =    -J5Z      ,    A4  =  -^^7(2.  1.  8)  appears  as: 


(2.  2.  4) 


(2.  2.  5) 


[A]     - 


0 

1 

0 

* 

0 

0 

0 

0 

0 

0 

0 

0 

fl  /\/f 

0 


AA 
f 


A, 


(2.1.  8) 


Solving  (2.  2.  5)  for  i    =    1 


_^. 


*i 

-A, 

0 

0 

0 

0 

0 

0 

'*H 

0 

1 

g21 

\ 

1 

g31 

0 

-Al+A4 

g41 

±=  0 


There  are  many  possible  solutions,   one  set  of  which  can  be: 


£         =    0 
e41 


g31     =    ° 


g 


21 


AtA^ 


g. 


AZ"A1 

A* 


11  Az-^  J 

for    i    =    2,    3,   4  respectively  (note  X„    =    0),   we  can  adopt: 


g42     =    ° 


g         =    0 

S32 


10 


g 


g 


22 


12 


Ax-Aj 


g43 

- 

0 

g33 

= 

-gK 

g23 

= 

0 

g13 

- 

0 

> 

g44 

= 

g/N 

g 


g 


g 


Then 


34 


g//v> 


2 
-A|   A-4- 


24        (A-A+XAi-A^ 


-A* 


14 


(Al-A+X^-Af) 


-A, 


1<*1 


-1 


A2"A|        7^-Aj 


A2-A2  Az-Aj 

0  0 


0 


1 


0 


Inverse  of    [g]  _±    is    [g]  : 
-1 
-1 


[°] 


1 
Ai 

X 

A2 


0 


0 
0 

± 

A+ 
0 


0 


0  -tF 


A_4 


0 


-a\ 


(a1-aa)(a2.-a4.; 

-Ai  A^. 


(A,-V)(Az-A+) 


a* 


A, 


4 


I 

A1A4. 

a 

A1-A4 

i 

A2.A4. 

3 

Ai-A^. 

l 

3 

M 

? 

(2.  2.  6) 
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By    (2.1.  6) 

X      =[A]x     +     £  U 

By  transformation 
1    =  [G]   x 
[G]_1i    =   [a][g]_1Z    +     cu 

i    =   LG][A]fG]_1X    +  [G]cu 

Define 


[gKaKg]"1  2f  J]- 


/V 


A. 


0 


0 


A 


A 


4- 


fd  1 

1 

r    ^1   l 

i-% 

d2 

A% 

j-X 

d 
3 

- 

2 

d4 

* 

[G].c    £     D    § 


Finally,   we  get  the  Jordan  Canonical  form  of  the  system: 
i    =  [J]  Z   +    Du 


(2.1.  6) 


(2.  2.  7) 


(2.  2.  8) 


(2.  2.  9) 


(2.  2.10) 
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2.  3    The  Reduced  System  of  Equations 

In  the  last  section,    the  whole  system  is  represented  by  equation 
(2.  2.  10).     Now  we  have  to  show  that  for  purpose  of  establishing  a 
controller  which  assures  stability  of  the  equilibrium  point  at  the  origin, 
it  is  sufficient  to  consider  the  following  reduced  system. 

y.    =  **>     y^;   +   d.  u  i    =    2, 3  (2.3.1) 

without  regard  for  the  coordinates    y1     and    y .  ,  which  associated 
with  negative  eigenvalues. 

If  this  is  true,   i.  e.  ,   a  controller    u    =    f  (y2,    yq)  can  be  found 
which  makes  the  system  (2.  3.  1)   asymptotically  stable  for  some  region 
about  the  origin  of  the  two  dimensional  space.     By  definition: 

y.  — ?  0       as    t — *■  <*=>  i    =    2,    3 

From  (2.  3.  1) 

(y.  -  dAi)  -*-  0         ast-xx?  i    =    2,    3 

Then,    certainly,    as    t  — *■  <x> 

jj  a-diujdt^o    **>    .  s  2  3 

But  -f-fAt  /  /f-Mt 


't 


Then  t+^ 

am 


t-**°) 


udt  — ►  0 


At^O 
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This  implies u  (t)  —^0    as  t—^-coin  the  sense  of  its  average 
value  over  any  very  small  non-zero  time  interval. 

Now,   we  come  back  to  those  equations  for    y1  and  y  .     By 
means  of  Jordan  canonical  form,   the  state  variables  are  express- 
ed by  partial  fraction  as  the  following  block  diagram. 

— >  y\ 


u 


d. 


S"-A. 


S  -An 


s  -x 


S  -X 


■>  y. 


-^  y. 


y 


FIG.    (2-3-1).     State  Variables  in  Jordan  Canonical  Form 


Let  h-  (t)  be  the  impulse  response  for  y.,   then 
Lim    y.  (At)  =  Lim        h.  (t-t   )    u  (t   )  dt 


r1-4 


Since  h.  (t)  approaches  zero  exponentially  as    t  —>  && 
(because  of  negative  eigenvalue),   and  since  as  mentioned  above, 
u  (t)  can  be  considered  to  approach  zero  under  any  integral  sign, 
it  follows  that  y.  — ?■  0    as    t— ?•  oo 


14 


Thus  the  initial  displacement  of  the  states  y    and  y      have 
no  effect  on  the  region  of  stability  of  the  complete  system.     There- 
fore,  from  now  on,  we  only  consider  the  reduced  system  described 
by  equation  (2;3sl). 


15 
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3.     Control  with  an  Analog  Computer 
3.  1    Selection  of  a  Controller 

A  controller  is  needed  to  provide  stability  in  the  region  of 
controllability,  which  means  the  largest  region  in  the  state  space 
within  which  the  system  can  be  brought  to  the  point  of  equilibrium 
at  the  origin  with  the  constraint    u4:U.     The  controller  will  be  a 
function  only  of    y2  and  y„  ,   but  through  the  transformation   y;=[C-JX 
it  will  generally  involve  all  the  state     variables  of  the  original 
system.    Pontryagin' s  maximum  principle  determines  an  optimum 
u  (t)    which  minimizes: 

y.<i3  =1  f<y2-  y3)dt 

with  the  constraint 

|u|  ^U 
and  final  states 

Yi  (f)  =    0  i    =    2,    3 

£e    is  some  positive  cost  function,   different  kinds  of  which 
lead  to  different  kinds  of  controller.     For  the  case  of  minimum- 
time  controller    \0  -    1.     We  define  a  new  state  variable: 

y4  =  y^>  '  I  dt 

It  follows 

y     =  i 

By  adding  this  new  state  variable,   our  system  gets: 

16 


y2 

'*2^2 

d2u 

y3 

=  •■ 

0 

+ 

u 

\ 

1 

0 

(3.  1.  1) 


Pontryagin  further  defines  the  Hamiltonian  function,   maxi- 
mizing this  H  function  with  respect  to    u    has  the  same  effect  as 
minimizing    yCfc). 
In  our  case: 

For  maximization  with  respect  to    u,   probably  the  bang- 
bang  type  controller  (u    =    _  U)  is  the  best  consideration. 
Let 

u    =    U  sgn  [  f '  J 

Usually  f '  can  be  derived  by  solving  the  following  canonical 
equations. 

P     =   _2H 

i       *yx 

In  this  problem,  it  is  hard  to  express  them  explicitly.  In 
view  of  piecewise  linear  switching,  the  following  guess  seems 
reasonable,. 


i   -  by3-y2 

where  b  is  some  positive  constant. 

17 


(3.  1.  2) 


Therefore 

u    =    U  sgnjby3  -  y2J  (3.  1.  3) 

If  there  is  no  control  on  cart  position,   that  means  ^  is  no  longer 

a  state  variable.     Then  by  equations  (2.  2.  1)  and  (2.  2.  6)  the 

uncoupled  state    y_    has  no  meaning.     In  this  case  the  whole  system 

(2.  3.  1)  reduces  to: 

y2    =  -^2^2  +  d2  u  *3'  1'  4* 

u    =    U  sgn[-y2]  (3.  1.  5) 

3.  2    Realization  of  the  Control 

Now,  we  get  a  minimum- time  controller,  which  is  (through 
the  functions  of  y2  and  yj  in  terms  of  the  original  state  variables, 
namely,    0p    9r>,   %^  and   £2.     Those  state  variables  can  be  gener- 
ated by  two  potentiometers    and  two  techometers.     We  select  a 
Donner  Analog  Computer  to  sum  them  up  and  use  a  D.    C.    relay  to 
generate  sgn  function.     The  real  structure  is  shown  as  Fig.  (3-2- 1) 
3.  2.  1    No  Control  of  Cart  Position 
By  (3.  1.  5) 

u    =    U  sgn(-  y2)  (3.  1.  5) 

By  (2.  2.  6) 

v        =   -    0      -     i-0      +  L^^±     C 

=  -  0     -  0.  241  02  -  0.  138  %z  (3.  2.  1.  1) 

18 


'IG.     (3-2-1).  eing  Co  i1  rolled  alo     Co     p     er 

10 


By  measuring  those  potentiometer  and  tachometers,   the 
following  data  are  obtained. 

9.        1  volt  corresponds  to  0.  087  radian 

0 

0.        1  volt  corresponds  to  40.  4    radian/ sec 

"^         1  volt  corresponds  to  0.  36  meter/ sec 
Multiplying  these  factors,  we  get: 

u    =    U  sgn  [  0.  087    Q1  +  9.  75   02  +  0.  05  %  2  J  (3.  2.  1.  2) 

where 


U    = 


0.  24 


(3.  2.  1.  3) 


The  circuitry  is  built  for  the  controller  as  shown  in  Fig.  (3-2- 1-1 ). 


fekf  Coil 


FIG.     (3-2-1-1).     Controller  Circuitry 


3.2.2    With  Control  of  Cart  Position 


By    (3.  1.  3) 


u    =    U  sgn  [by3  -  y2  ] 


(3.  1.  3) 
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From  (2.  2.  6) 

y0  = 


1  e-j +  lh^  =  °'204V  °'  102^2 


? 


? 


(3.  2.  2:  1) 


For    ^  . 

1  volt  corresponds  to  0.  05  meter 

Then 

y      =    0.  0102  |t+    0.  036f 
J3  *  2 

The  following  network  is  added  to  the  connector  in  Fig.  (3-2-1-1) 


1M 
■vWW 


?i" 


To  ConNec-tor 
°f   fl% (3-2- 1-1) 


FIG.     (3-2-2-1).     Additional  Network  for  Position  Control 

If  the  value  of    b    is  not  big  enough,   the  cart  has  a  tendency 
to  settle  itself  a  little  bit  off-center.     For  instance,   taking    b=0.  08 
(as  shown  in  Fig.    3-2-2-1),   the  cart  tends  to  settle  itself  about  0.  2 
meter  from  its  starting  position.     If  the  polarity  of  the  potentio- 
meter for  £  is  reversed,   the  cart  will  tend  to  settle  in  the  opposite 
side  from  the  starting  position.     Anyhow,   if    b    is  big  enough,   the 
cart  will  come  back  to  its  starting  position.     This  is  shown  in 
appendix  II- 5,   for    b  =  0.  7. 
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4.     Simulation  by  Graphs 

4.  1    The  Region  of  Controllability 

When  we  are  playing  with  the  physical  cart,   we  can  start  the 
cart  motion  by  just  pushing  the  pendulum  off  its  vertical  position. 
Now,   for  the  case  of  simulation  with  digital  computer,  we  encounter 
the  problem  of  deciding  the  initial  condition  of  0..     In  other  words, 
we  want  to  know  the  region  of  controllability  which  means  the 
largest  region  in  the  state  space  from  which  the  system  still  can 
be  brought  back  to  its  equilibrium  point. 

With  a  bang-bang  type  controller  (u  =  _  U),   the  trajectories 
consist  of  two  segments  corresponding  to    u  =  U  and  u  =  -U 
respectively.     The  origin  of  the  two-dimensional  space  can  always 
be  reached  by  this  switching  of  u.     The  region  of  controllability, 
then,   can  be  defined  as  the  set  of  points  reachable  by  the  trajec- 
tory starting  at  the  origin  (y^  =0,   y3  =  0),   and  proceeding  in 
reverse  time(0^t  ^.-  co)  with  u  alternately  taking  values  of  +U 
and  -U.     In  our  problem: 

^2  =A2  y2  +  d2u 


(2.  3.  1) 


y3  -  d3u 


Those  first  order  differential  equations  can  be  easily  solved 
with  solutions  as: 
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y2  +  (d2u/7v2)  r      ,a    .  a 

-2 ?_      =    exp|A2(t-tJ  (4.1.1) 

y20  +  (d2U/^) 


-^r — ^    =    t  -  t  (4.  1.  2) 

dgll  O 

From  (4.  1.  2),   y„  is  undefinted  as  t  ->  -<»,    no  matter  what  the 
initial  value  y       is.     Thus  the  region  of  controllability  is  unbounded 
in  the  y3  coordinate.     Equation  (4.  1.  1)  shows  directly  that 


y- 


d2v2 


H<f; 


as  t  -*■  -  CO ,   hence  y2  must  be  bounded  by: 


'2 

Numerically  we  have 

|y2|<.0.45  (4.1.3) 

By  (2.  2.  6) 

v     =_0_1     q+L^±- 
y2         9      ^^JVA^. 

If  only  9  has  non-zero  initial  value,   it  must  be  bound  as: 

0  (O)U^Co.  45 

This  is  the  reason  of  using  0.  1  (radian)  as  the  initial  value  of 
angle  displacement. 
4.  2    Analysis  by  Graphs 

4.  2.  1    No  control  on  Cart  Position. Equation  (2.  1.  5)  can  be 
written  as: 
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X- 


X, 


X, 


X, 


0  1 

17.  2  0 

0  0 

0  0 


0  0 

0  3.  5 

0  1 

0  -^2 


',       s. 

r                     N 

xl 

0 

x2 

-17.  2 

X 

+ 

X3 

0 

0 

A 

9.  8 

X  u       (2.  1.  5) 


By  (3.  1.  5)  and  (3.  2.  1.  1): 

u    =    0.  24  sgn  fx1  +  0.  241  x2  +  0.  138  x4]  (4.2.1.1) 

Now,   this  system  can  be  simulated.     For  better  accuracy,  we 
use  sin  x^  instead  of  x^.     Graphs  are  shown  in  Appendix  I.     Same 
initial  angle  for  all  graphs. 

Fig.    1-1  shows  pendulum  angle  vs.    time,  with  initial  angle 
displacement  0.  1  radian.     It  comes  back  very  quickly. 

Fig.   1-2  shows  the  pendulum  changing  rate  vs.    time. 

Fig.    1-3  shows  the  cart  position  vs.    time. 

Fig.   1-4  shows  the  phase  plane  (0  vs.    9).     The  tracjectory 
does  come  back  to  the  origin.     It  means  stability. 

» 

Fig.    1-5  shows    Zvs.  2.  .     As  time  goes  on,   the  position 
rate  decreases  linearly  to  zero. 

Fig.    1-6  shows  the  control  force  vs.    time.     It  is  the  bang- 
bang  type  force;  only  the  direction  of  the  force  is  switched  by  the 
relay. 
4.  2.  2    With  Control  on  Cart  Position. 

In  this  case,   the  system  equations  are  same  as  before,   but 
the  control  force  changes  as: 
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u    =    0.  24  sgn[x1+ 0.  241x2+(0. 102b+0.  138)x4+0.  204bx3l 

We  simulate  this  system  with  the  initial  angle  displacement 
as  before  (0  (o)  =  0.  1  radian).     Graphs  are  listed  in  Appendix  II. 

Firstly  with  b  =  0.  1  for  position  control. 

Fig. II- 1  shows  pendulum  angle  vs.    time.     It  is  stable. 

Fig.  II- 2  shows  the  phase  plane.     The  tracjectory  goes  to  the 
origin. 

Fig.  II- 3  shows  the  cart  position  vs.    time.     It  does  not  come 
back  very  quickly,   because  the  amount  of  b  is  too  small. 

Then  take  b  =  0.  7  for  position  control. 

Fig.  II-4  shows  the  stick  angle  vs.    time. 

Fig,  II- 5  shows  the  position  vs.    time.     The  cart  comes  back 
to  where  it  started  very  quickly. 
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5.     Optimal  Discrete -time  Control 

5.  1    The  System  in  Discrete-time  Form 

Recall  the  original  system  as  follows: 

x  =  (A]  x  +  c  u  (2.  1.  6) 

Consider,   firstly,   the  equation  without  control  force. 

x  =   (A]  x  (5.  1.  1) 

and  assume  a  Taylor  series 

x  (t)  =  A0  +  A:t  +  A2t2  +.  .  .  +Amtm+.  .  .  (5.  1.  2) 

to  be  the  solution  of  the  above  homogeneous  differential  equation. 

Then,    set  t  =  0,   one  obtains: 

x  (0)  =  A 
—  o 

Next,   if  (5.  1.  2)  differentiated  and  then  t  set  to  zero,   one  obtains: 

x  (0)  =  Ax 
But,   from  (5.  1.  1) 

x  (0)  =  [A]  x(0) 
Then 

A1  =03  x  (0) 

If  (5.  1.  2)  is  differentiated  twice,   and  t  set  to  zero,   one 
obtaines: 

x  (0)  =  2A2 
or 

2A2  =  x  (0)  =  (A)  x  (0)  =  [A]  2  x  (0) 
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So  that 

A2  =  1/2W22£(0) 

Continuing  this  process,   all  the  terms  A-  can  be  evaluated, 

and  one  obtains: 

x  (t)  =  |  [i]  +f  A]  t  +C£i  t        +•  •  •  +  CAl"1^  +...U(o)         (5.  i.  3) 
I  2  f  m  I  / 

at 
By  comparing  it  with  the  scalar  expansion  of  e     ,   it  is  obvious 

to  have  a  more  compact  form,  like: 

x  (t)  =  eAt  x  (0)  (5.  1.  3.  1) 

In  imy  case,   eA*  is  a  4  x  4  matrix.     It  is  usually  called  funda- 
mental matrix  and  designated  by: 

(J  (t)  £>e  At 
Apparently 

*(-t>  =  e_At=-«r7TT-=-f1(t)  (5-1-4) 


,  TO 

Also 

X  (t)  =  f  (t)  x  (o) 
Then 

x  (t)  =  0  (t)  x  (o)  =  (A]  x  (t)  =  [A)  </)  (t)  x  (o) 
So  that 

\  (t)  =  (>]  /  (t)  (5.  1.  5) 

In  order  to  solve  the  equation  (2.  1.  6),  we  try  to  find  a  particu- 
lar integral  in  the  form  of 
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x       (t)  =  p  (t)  y  (t) 

-p  r 

By  putting  into  (2.  1.  6),    one  obtains 

0  (t)  y  (t)  +  j?l  (t)  y  (t)  =  [A]  jf  (t)  y  (t)  +  c  u  (t) 
By  (5.  1.  5) 

$  (t)  y  (t)  =  cu(t) 

y(t)  =  f  f\r)C  U(rC)dZ 
Then 

xp(t)  =f>  (t)/    i>     (TJ*  u(T)dZ 
By  (5.  1.4) 

xp  (t)  =  j>  (t)|  4(-V±  U(Z)dT  (5.  1.  6) 

In  evaluation  of  0  (t),   we  know  the  argument  t  represents  the 
time  interval  between  two  instants.     More  conveniently,   we  can 
describe  by: 

x  (t2)  =  f  (t2  -  t%)  x  (t2) 

x  (t3)  =  f  (t  3  -  t2)  x  (t2)  =  >f  (t  3  -  t  x)  x  (tx) 

But  XLii)^i>(trtx)^(t1,''tl)X(t1) 

t^W'f  (t3~  t2>'  /<*2-  *1> 

By  this  reason,   (5.  1.  6)  can  be  put  into  the  more  familiar  form  of 

a  convolution  integral. 

x   (t)  =    P/f  (t  -T)  c  u  (T)  dT  (5.  1.  6.  1) 

P  /<> 

Then  the  general  solution  of  (2.  1.  6)  will  be: 


x  (t)  =  </>  (t)  x  (o)  +ftf  (t-r)cu  (r )  cr 

'o 

In  case  of  discrete  time,   it  turns  out  to  be: 


(5.  1.  7) 


x  (k+1)  =  f  (DT)  x  (k)  +/DT/  (DT  -£)  c  u  ( Z  )  dZT  (5.  1, 


8) 


Where 


DT  =  sampling  time 

By  noting  of  a  constant  control  force  through  the  interval  DT, 
one  obtains: 

x  (k  +  1)  =  0  (DT)  x  (k)  +  u(k)^  /  DT  fi  (DT-r  )cdr  (5.  1.  9) 

Or 

x  (k  +  1)  =  jjf  (DT)-    x  (k)  +^  u(k)  (5.  1.  10) 

Where 


(dt  -n  c  dr 


r     'Z  2  r    _m  m 

6  (DT)  =  [  I]  +[A]  •    DT  +    IAWDT1+-  ■  •  +[£     (DT)     +•  •  • 

2  J  m.' 

The  computation  of  A  (DT)  and  0  (DT)  would  be  very  tedious, 
but,   with  the  high  speed  computer,   they  can  be  obtained  within 
seconds.     The  program  to  achieve  this  is  shown  in  Appendix  IV- 3. 
(DT  =  0.  1  sec.  ) 


Where 


-0.  0817 

-1.  6067 

0.  0458 

0.  8882 
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1.  0872  0.  1029         0.  0000         0.  0166 

1.  7697  1.  0872         0.  0000         0.  3268 

0.  0000  0.  0000         1.  0000         0.  0906 

0.0000  0.0000         0.0000         0.8187 

5.  2    Evaluation  of  Control  Force, 

In  optimal  discrete-time  control,   if  we  want  to  minimize 
the  following  cost  function. 

(5.  2.  1) 
The  second  term  represents  the  amount  of  control  force 
which  can  be  allowed,   arbitrarily  r  set  to  1.     The  first  term 
gives  the  choice  of  state  variables  which  will  be  minimized.    In 


J(n)  =£  fx1  (k)  Q  x  (k)  +  ru2  (k-1)] 


my  case,   x1  and  Xo  are  those  variables.     So,   Q  becomes: 

[Q] 


\lo      0 

0  \ 


By  (5.  1.  10) 


(5.2.  1) 


J(n)  ^^(n-D+Aufn-l)!1  QWn-1)  +Au(n-  1)1  +ru2(n-  1)  +  J(iS-l) 

Minimizing  with  respect  to  u(n-l),      ,*Qd =  0     and  noticing 

^u(n-l) 

J^n-1)  is  independent  of  u(n-l),   gives 

jx*  (n-1)  0t+u(n-l)A1/QA+AtQJ^x  (n-1)  +4u(n-  l)/+2ru(n-l)  =  0 


u(n-l)  =  --T 


AtQ^T 


ALQA  +  r     - 


x  (n-1) 


(5.  2.  2) 
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Define 

a*    P  -  £  S  I  (5.2.3) 

1  A1  QA+r 

Then 

u(n-l)  =  a'  x  (n-1)  (5.  2.  2.  1) 

Substitute  u(n-l)  to  (5.  2.  1),    one  obtains:  (5.  2.  4) 

J^n)  =\}[i  x  (n-2)+Au(n-2)J  +4a/[    IpQ^f    J+aa^jj 
+  r[     faja^f        ]+[     ] t  Q  [     ]+ ru2(n-2)  +  J(n-2) 
Where  a.   =  (a,*)1" 

[      ]P[j^x  (n-2)  +£u  (n-2)J 
Further  defining 

-^  P  0  +Aa1t  (5.  2.  5) 

The  first  and  third  terms  of  (5.  2.  4)  can  be  combined  as: 

([    ]***  +  [    jta^JQf^    ]+Aaa*C    ]]  +  £    JtQC    ) 
=  [    ^(^Q/f +/tQ^d-1t+a1^1Q/  +  a1AtQ4a1t)[    ]  +  C  J  *  QC  J 

=  C     Jt(j^tQ<^+^a1t>+a1AtQ<^+Aa1t>)[    J  +  f  ]*  Qf  J 
=  C     )t(0t  +  alA1)Q()?J4:^alt)(     J+(    J  *  Q[  J 

=  C   d^q^x:  3  +C  ]*q£  J 

Now,    (5.  2.  4)  becomes: 

J(n)  =  L     ]  tty1tQf1  +  Q)f     j+r/:    J  ta1a1t  f  J  +  ru2(n-2)  +  J(n-2) 
=  1    ]  t(^1t  Qyx  +  Q+  rajaj  )[    ]  +  ru2(n-2)  +  J(n-2) 
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Define 

Pj    =  yx   Qf1+  Q  +  r  a1a1  (5.2.6) 

dJlnl       =  0  gives: 
^u(n-2) 

(    ]l  p^+A^jC    ]  +  2  ru(n-2)  =  0 
(xt  (n-2)  01  +  u  (n^^Jp  A+^p    L     ]   +  2  ru  (n-2)  =  0 
u  (n-2)  =  -     f  h?        x  (n-2)  (5.  2.  7) 

Define 

a2=-A*p^T7  <5-2-8> 

Then 

u(n-2)  =  ag*  x  (n-2)  (5.2.7,1) 


(5.  2.  3)  becomes: 


Define  P    -T  Q 


a.t  =  .^_  (5.2.3.1) 


1  A1  p  A  +r 


o 


Continuing  one  more  stage,   and  set         .        .  =  0,   one  obtains: 

^u(n-3) 

y2  =j2f+^a2t 

P2  =  ^2   P  1 V^  +  Q  +  r  a  2s  2 

3  AtP2^+  r 

u(n-3)  =  a^    x  (n-3) 

Continuing  on  the  same  procedure,   one  can  expect  the 
following  general  forms. 
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A  -  i  +A*k 
pk=Vpk-iy'k  +  ,3  +  rakakt 

t    a'V,  i 
ak'/fpk-lA+r 

u(n-k)  =  a,     x  (n-k) 

k   ~~ 

We  note  that  u(n-k)  depends  solely  on  those  present  states 
x  (n-k).     Thus  makes  clear  Bellman's  "Principle  of  Optimality", 
which  states:  "An  optimal  policy  has  the  property  that  whatever 
the  initial  state  and  the  initial  control  input  rector  are,   the 
remaining  control  input  rectors  must  constitute  an  optimal  policy 
with  regard  to  the  state  resulting  from  the  first  control  signal.  " 

When  the  number  of  stages  gets  very  large,  a  converges 
to  some  final  set  of  values.  Thus  exists  some  fixed  values  for 
the  feedback  compensation  for  all  values  of  time  and  state  variables. 

With  the  help  of  CDC  1604  computer,    200  stages  are  accom- 
plished.    The  program  is  shown  in  Appendix  IV-4.     Finally,   one 
obtains,   for  all  sampling  time: 
2.  4846 


u(t)  = 


0.  5990 
0.  0074 
0.  3448 


x(t) 


(5.  2.  9) 
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5.  3    Simulation  by  Integration 

With  the  control  force  shown  in  (5.  2.  9),   we  can  simulate 
the  system  by  calling  a  subcontine  for  integration.     The  control 
force  is  calculated  during  every  sampling  instant  and  hold  con- 
stant until  the  next  sampling  instant. 

The  FORTRAN  program  achieving  this  purpose  is  shown 
in  Appendix  IV- 5.     Initial  angle  displacement  is  same  as  before, 
x  (1)  =  0.  1  radian.     Sampling  time  DT  =  0.  1  second. 

The  graphs  are  collected  in  Appendix  III. 

Fig.    Ill- 1  shows  stick  angle  vs.    time.     The  angle  comes 
back  after  about  40  stages. 

Fig.    Ill- 2  shows  phase  plane. 

Fig.    Ill -3  shows  control  force  vs.    time. 

Fig.   III-4  shows  cart  position  vs.   time.     It  gets  back  to  the 
starting  position  comparatively  slowly. 
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APPENDIX  I 
Graphs  for  No  Control  of  Cart  Position 

1.  Stick  Angle  vs.    Time  Without  Position  Control 

2.  Stick  Angle  Changing  Rate  vs.    Time  Without  Position  Control 

3.  Postion  vs.    Time  Without  Position  Control 

4.  Phase  Plane  Without  Position  Control 

5.  Position  Changing  Rate  vs.    Position  Without  Position  Control 

6.  Control  Force  vs.    Time  Without  Position  Control 
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FIG.     (1-1).     Stick  Angle  Vs.    Time  Without  Position  Control 
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APPENDIX  II 
Graphs  for  Control  of  Cart  Position 

1.  Stick  Angle  Vs.    Time  With  Less  Position  Control  (b  =  0.  1) 

2.  Phase  Plane  With  Less  Position  Control  (b  =  0.  1) 

3.  Position  Vs.    Time  With  Less  Position  Control  (b  =  0.  1) 

4.  Stick  Angle  Vs.    Time  With  More  Position  Control  (b  =  0.  7) 

5.  Position  Vs.    Time  With  More  Position  Control  (b  =  0.  7) 
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APPENDIX  III 
Graphs  for  Discrete-time  Control 

1.  Stick  Angle  Vs.    Time  for  Discrete-time  Control 

2.  Phase  Plane  for  Discrete-time  Control 

3.  Control  Force  Vs.    Time  for  Discrete-time  Control 

4.  Position  Vs.    Time  for  Discrete-time  Control 
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APPENDIX  IV 
FORTRAN    Programs 

1.  FORTRAN  Program  for  Simulation  without  Position  Control 

2.  FORTRAN  Program  for  Simulation  with  Position  Control 

3.  FORTRAN  Program  for  Calculating   (f)   and  A  Matrix 

4.  FORTRAN  Program  for  Calculating  Discrete-time  Control  Force 

5.  FORTRAN  Program  for  Simulation  of  Discrete-time  Control 
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.•JOB0250F,WAN 

PROGRAM  BROOM 
C      NO  CART  POSITION  CONTROL 

DIMENSION  X(30) »XDOT(30)fC(15)  ' 

C<10)=1.0 
1  CALL  JNTEG1  <T»X»XDOT»C) 

X0OT(l)*X(2) 

XDOT(2)»17«2*SINF(X( 1) >+3.5*X< 4)-17.2*U 

XDOT(3)=X<4) 

XDOT  <  4 ) »-2 . 0*X ( 4 ) +9. 8*U 

U=0.24*SIGNF(1.»X(1)+C(1)*X(2)+C(2)*X(4)) 

X(5)»U 

GO  TO  1 

END 

END 
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PROGRAM  BROOM 
C      POSITION  CONTROL   . 
C      RUN  NO.  TWO  FOR  MORE  Y3  FEED  BACK 

DIMENSION  X( 30)  ,XDOT (30)  »C(  15) 

C( 10)=1. 
1  CALL  INTEG1  (T,X,XDOT,C) 

XDOT( 1) =X(2) 

XDOT(2)=17.2*SINF(X(  1  )  )  +3  .  5*X  (  4  )  -1  7  .2*1) 

XD0T.(3)=X(4) 

XDOT(4)=-2.*X(4)+9.8*U 

U=0.24*SIGNF( l.»X(l)+C(l)*X(2)+(C(3)*0.102+C<2)) 
1*X(4)+C(3)*0.204*X(3) ) 

X( 5)=U 

GO  TO  1 

END 

END 
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..JO30250F,WAN 

PROGRAM  PHIOEL 
C      COMPUTING  PHI  AND  DELTA  MATRIX 
C      DT=SAMPLING  TIME 

DIMENSION  A(12,12),PHI(12,12)»TERM(12,12)  »w/ORM( 12»12  ) 
1 ,DEL(4) ,DELM(4,4),TELM(4,4),DELP(4,4),C(4) 

N  =  4 


1 

1003 
399 

3991 


DT=0.1 
TM=0.0 
READ1. 
FORMAT 
READ  1 
PRINT 


( (A( IR, IC) , IC=1,N) , IR=1»N) 

(  (4F20.8)  ) 
, (C( I ) ♦ 1=1, N) 
3  99,DT,((A(IR,IC),IC=1,N),IR=1,N) 


FORMAT ( ///1X,3HDT=,1F5.3///,1X,7HA( I,J)=/,((4F10.2))) 
PRINT  3991  (C(  I.)  ,I=1,N) 
FORMAT ( ///1X,5HC(I)=/(4F10.2)) 


400 


DO  400  IR=1,N 
DO  400  IC=1»N 
TERM( IR, IC) =0.0 
WORM( IR , IC) =0.0 
TERM{ IR,IR) =1.0 
TELM(IR,IC)=TERM(IR, IC)*DT 
DFLP( IR,IC)=TELM( IR, IC) 
.DELM(  IR  ,  IC  )  =0.0 
DEL( IR)=0. 
PHI  (  IR,  IC)=TERM(  IR , I C ) 


4  TM=1.0+TM 

DO  5  00    IR=1»N 

DO  500    IC=1,N 

DO  500    JN=1,N 

DELM( IR,IC) =DELM( IR, IC )-TELM( IR,JN)*A( JN, IC)*DT/( TM+1 
50  0  WORM( IR,IC) =TERM( I R , JN ) *A ( JN , I C ) *DT/TM+WORM ( IR, IC) 

DO  401  IR=1,N 

DO  401  IC=1,N 

TERM( IR,IC) =WORM( IR, IC) 

TFLM( IR , IC) =DELM( IR, IC) 

DELP( IR,IC) =DELP( IR, IC)+TELM( IR, IC) 

PHI ( IR,IC)=PHI { IR,IC)+TERM( IR,IC) 

DELM( IR , IC) =0. 
40  1  WORM( I R, IC) =0.0 

ABC=0.0 

DO  2  I=1,N 

DO  2  J=1,N 

AA=TERM( I ,J) 

AB=ABSF(AA) 

IF(ABC-AB)  3,3,2 

FIND  BIGGEST  VALUE 
3  ABC=AB 
2  CONTINUE 

IF(0.000000005-ABC)  5,5,6 

5  GO  TO  4 

6  PRINT  502, (  (PHI ( IR,IC)  ,IC=1,N)  ,IR=1,N) 


50  2  F0RMAT(///1X»9HRHI ( I » J  )  =/ (  ( 4F15  .9 )  )  ) 


DO  600  1=1, N 

DO  .6  00  K=1»N 

DO  600  J=i ,m   • 

DEL( I  )=DEL(  I )+PHI  (  I , J ) *DELP ( J ♦ K ) *C ( K  ) 

PRINT  503  (DEL< I ) ♦ 1=1. N) 

FORMAT (/// IX »7HDEL(  I)=//(4F15.9)) 

END  | 

END 


600 
503 


i  I 


DT=0.10 

A(  I  ,  J)  = 

.00 

1.00 

.00 

.00 

17.20 

.00 

.00 

3.50 

.00 

.00 

.00 

1.00 

.00 

.00 

.00 

-2.00 

C(  I  )  = 

.00 

-17.20 

.00 

9.80 

PHI ( I ,J)= 

1.087239756 

.  '1.769732445 

0.000000000 

0.000000000 


0.102891421 
1.087239756 
0.000000000 
0.000000000 


0.000000000 
0.000000000 
1.00000000 
0.00000000 


0.016631936 

0.326856101 

.090634623 

.818730753 


DEL(  I  )  = 

-.081750092 


-1.606739468 


0.045890345 


0.888219310 
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1=1 »N) 
,I=1,N) 


44 


45 


46 


47 


C 
C 


JOB0250F,WAN 

PROGRAM  OPTCON 

MINIMUM    J=SUM(XT(K ) *Q*X ( K ) +R*U**2 ) 

DIMENSION    PHI(4,4),PSI(4»4),P(4,4)  , DEL(4)  .AT (20,4  )  , 
1 GM ( 4 , 4 )  , Q ( 4  ,  4  )  ,  FM ( 4 )  ♦ EM ( 4 ) 
REAlS    1 »N,NM,NPRINT 

1  FORMAT  (8110) 
READ2»R»DT 

READ2,  ( (Q( I ,J) ,J=1,N) 
READ2> (  (PHI (  I  ,J)  ,J=1»N 
REA62, (DEL( I ) , 1=1, N) 

2  FORMAT ( (4F16.9) ) 
PRINT  3,N,NM,NPRINT 

3  FORMAT(///(8I10) ) 
PRINT  44,R,DT 

F0RMAT(//1X,2HR=,F5.2,3X,3HDT=,F5.2) 
PRINT  45, (  (Q(  I , J)  , J=1,N)  ,I=1,N) 
FORMAT (// IX, 7HQ( I»J)=/((4F16.9))) 
PRINT  46,  (  (PHI  (  I ,J)  ,J  =  1  ,N) , 1  =  1, N) 
F0RMAT(//1X,9HPHI (I,J)=/((4F16.9))) 
PRINT  47,  (DEL( I ) , 1=1, N) 
FORMAT (// IX, 7HD EL ( I)=/(4F16.9) ) 
DO  5I=1,N 
D05  J=1,N 
GM(  I  ,J)=0.0 
EM(  I  )=0.0 
FM( I )=0.0 
P( I ,J)=Q( I , J) 
PSI ( I ,J)=0.0 

CALCULATE   AT(K,J) 

DO  22  KK=1,NPRINT 

DO  2  0  K  =  1,,NM 

DEN  =  0..0 

D06  1=1, N 

DO  6  J=1,N 

EM(  I  )=EM(  I )+DEL( J)^P( J, I ) 

DO  8  I=1,N 

DO  7  J=1,N 

FM(  I  )=FM(  I  )+EM(  J)*PHI.(  J»I  ) 

DEN=DEN+EM( I )*DEL( I ) 

DEN=-DEN-R 

DO  10  1=1, N 

AT(K, I )=FM(  I  )/DEN 

FM( I )=0.0 

EM(  I  )  =  0.0 


CALCULATE    PSI(K,I,J) 
DO  13  1=1, N 
DO  13  J=1,N 
PSI(I»J)=PHI(I,J)+DEL(I)*AT(K»J) 


10 


13 


CALCULATE 
DO  16  1=1, N 


P(K,I ,J) 
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15 


16 
20 


12 
22 


DO  16  J=1,N  .    • 

DO  15  L=1»N 

DO  1.5  M=1»N 

GM(  I  »J)=GM(  I  ,J.)+PSI ( L» I  )*P(L.M1*PSI  (Mi  J) 

P(  I  ,J)=GM(  I  ,J)+R*AT(K,I }*AT(K»J) 

FOR  TERMINAL  CONTROL  OMIT  Q(I.J) 

GNU  I » J) =0.0 

CONTINUE 


PRINT 

PRINT  12»KK»(AT(NM»J)»J=1»N) 

FORMAT (// IX, 3HAT( ,I2,2H)=/(4F16 

CONTINUE 

END 

END 


9) 


AT( 10)= 

2.484604417 


•5990829066 


•007459609 


.344834834 
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. . JOB0  250F,WAN 

PROGRAM  BROOM 
C      OPTIMAL  DISCRETE  TIMF  CONTROL 
C      FOR  SIMPLICITY.  USE  AT(J)  IN  LAST  STAGE  FOR  ALL  U 

DIMENSION  X(30) ,XDOT (30) *C( 15) 

C  (  1  3  )  =  1 . 

1  CALL  INTEG1  (T,X,XDOT,C) 
XDOT<(  1  ) =X(2) 

XDOT(2) =17.2*SINF(X( 1) )+3. 5*X ( 4 )-17. 2*U 

XDOT(3) =  X(4) 

XDOT( A) =-2.*X(A)+9.8*U 

DT=0.1 
C      DT  =  HOLD  TIME 

IF(T-C(5))  1,2,2 
C      ABOVE  STATEMENT  AS  A  ZERO  ORDER  HOLD 

2  U  =  C(  1)*X( 1)+C(2)*X(2)+C<3)*X(3)+C(4)*X(4) 
X(5)=U 

C(5)=C(5)+DT 
GO  TO  1 
END 
END 
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